#include "control_base/WMJRobotControl.h"
#include <bits/stdint-intn.h>
#include <cmath>

namespace wmj
{

    /**
     * @brief: 获取当前能量机关状态
     *
     * @retrun: wmj::ROBO_ENERGY ENERGY_IDLE 0x00不可激活, ENERGY_SMALL 0x01小能量, ENERGY_BIG 0x02大能量
     */
    ROBO_ENERGY WMJRobotControl::getEnergyStatus()
    {
        Buffer data_from_read = readFrame(Judge);
        return (ROBO_ENERGY)(data_from_read[0] & 0x03);
    }

    /**
     * @brief: 接收弹速回传计算平均值用以弹道补偿的回传
     *
     * @return: float 弹速
     */
    float WMJRobotControl::getShootSpeedValue()
    {
        if (canBus->canUseThisPort)
            canBus->m_clear_shoot_speed = true;
        else if (canBus_spare->canUseThisPort)
            canBus_spare->m_clear_shoot_speed = true;

        Buffer data_from_read = readFrame(Judge);

        ROBO_SHOOT shoot_level = (ROBO_SHOOT)((data_from_read[0] & 0x0C) >> 2);
        float f_shoot_speed = 0;
        f_shoot_speed = ((int16_t)(((int16_t)data_from_read[1]) | ((uint16_t)data_from_read[2] << 8))) / 10.f;

        // 等级切换了 现在算出的弹速是上一等级的 弃用并清空队列 返回默认值
        // 为了读取实时弹速 Buffer会清空 设置判断条件SPEED_IDLE
        if (m_shoot_level != shoot_level && shoot_level != wmj::ROBO_SHOOT::SPEED_IDLE)
        {
            m_prev_shoot_speed.clear();
            m_shoot_level = shoot_level;
            m_shoot_speed_initialized = false;
            if (m_shoot_level == SPEED_HIGH)
            {
                return m_thirty_shoot_speed;
            }
            else if (m_shoot_level == SPEED_MID)
            {
                return m_eighteen_shoot_speed;
            }
            else
            {
                return m_fifteen_shoot_speed;
            }
        }

        // 回传不是0
        if (f_shoot_speed > 0)
        {
            m_shoot_speed_initialized = true;

            m_prev_shoot_speed.push_back(f_shoot_speed);
            if (m_prev_shoot_speed.size() == 11)
            {
                m_cur_shoot_speed = (10 * m_cur_shoot_speed - m_prev_shoot_speed.front() + m_prev_shoot_speed.back()) / 10.f;
                m_prev_shoot_speed.pop_front();
            }
            else
            {
                m_cur_shoot_speed = (m_cur_shoot_speed * (m_prev_shoot_speed.size() - 1) + f_shoot_speed) / m_prev_shoot_speed.size();
            }
            return m_cur_shoot_speed;
        }
        else // 等级切换后弹速回传仍是0
        {
            if (m_prev_shoot_speed.size() > 0)
                return m_cur_shoot_speed;
            if (m_shoot_level == SPEED_HIGH)
            {
                return m_thirty_shoot_speed;
            }
            else if (m_shoot_level == SPEED_MID)
            {
                return m_eighteen_shoot_speed;
            }
            else
            {
                return m_fifteen_shoot_speed;
            }
        }
    }

    /**
     * @brief: 返回当前等级的最高弹速
     *
     * @return: float
     */
    float WMJRobotControl::getMaxShootSpeed()
    {
        if (m_shoot_level == SPEED_LOW)
        {
            return m_fifteen_shoot_speed;
        }
        else if (m_shoot_level == SPEED_MID)
        {
            return m_eighteen_shoot_speed;
        }
        else
        {
            return m_thirty_shoot_speed;
        }
    }

    bool WMJRobotControl::getVirtualShield()
    {
        Buffer data_from_read{};
        return (bool)(data_from_read[0] & 0x20);
    }

    ROBO_GAIN WMJRobotControl::getRobotGain()
    {
        Buffer data_from_read{};
        return (ROBO_GAIN)(data_from_read[0] & 0xc0);
    }

    ROBO_RFID WMJRobotControl::getRobotRFID()
    {
        Buffer data_from_read{};
        return (ROBO_RFID)(data_from_read[1] & 0x03);
    }

    int WMJRobotControl::getLeftHP()
    {
        Buffer data_from_read{};
        return (uint16_t)data_from_read[2] | ((uint16_t)(data_from_read[3] & 0x0f) << 8);
    }

    int WMJRobotControl::getBaseHP()
    {
        Buffer data_from_read{};
        return (uint16_t)(data_from_read[3] >> 4) | ((uint16_t)(data_from_read[4]) << 4);
    }

    int WMJRobotControl::getOutpostHP()
    {
        Buffer data_from_read{};
        return (uint8_t)data_from_read[5];
    }

    GameStateMsg WMJRobotControl::getGameState()
    {
        GameStateMsg game_state_msg;
        Buffer recv_data_state = readFrame(State);
        Buffer recv_data_judge = readFrame(Judge);
        Buffer recv_data_key   = readFrame(MainControl);
        Buffer recv_data_client = readFrame(Client);

        if (recv_data_state.size() > 0)
        {
            // game_state_msg.self_check = (recv_data_state[0] >> 5) & 0x01;
            game_state_msg.client_key = uint8_t(recv_data_state[1]); 
            game_state_msg.bullet_remain= uint16_t(recv_data_state[0] << 1);
            game_state_msg.goal_position_y = uint8_t(recv_data_state[2]);
            game_state_msg.enemy_alive = uint8_t(recv_data_state[3]);
            game_state_msg.defence_buff = int8_t(recv_data_state[4]);
            game_state_msg.current_hp = (uint16_t(recv_data_state[5])) | (uint16_t(recv_data_state[6] & 0x0F) << 8);
            game_state_msg.goal_position_x = (uint16_t(recv_data_state[6] >> 4) & 0x0F) | (uint16_t(recv_data_state[7]) << 4);
            game_state_msg.enemy_outpost_status = uint8_t(recv_data_state[3] >> 6) & 0x01;
            // game_state_msg.max_hp = (uint16_t(recv_data_state[2] >> 4) & 0x0F) | (uint16_t(recv_data_state[3] ) << 4);
            // game_state_msg.level = (recv_data_state[4] >> 4) & 0x0F;
            // game_state_msg.recovery_buff = recv_data_state[6];
            // game_state_msg.bullet_remain = (uint16_t(recv_data_state[4] >> 4) & 0x0F) | (uint16_t(recv_data_state[5]) << 4);

            // game_state_msg.game_state = recv_data_state[0] & 0x01;
            // game_state_msg.outpost_state = (recv_data_state[0] >> 1) & 0x01;
            // game_state_msg.RFID_patrol = (recv_data_state[0] >> 2) & 0x01;
            // game_state_msg.RFID_elevated = (recv_data_state[0] >> 3) & 0x01;

            // game_state_msg.armor_id = recv_data_state[1] & 0x07;
            // game_state_msg.harm_type = (recv_data_state[1] >> 3) & 0x03;
            // game_state_msg.sentry_blood = uint16_t((recv_data_state[1] >> 6) & 0x03) | (uint16_t(recv_data_state[2]) << 2);

            // game_state_msg.bullet_remain = recv_data_state[3];


            // game_state_msg.hityaw = uint16_t(recv_data_state[6]) | (uint16_t(recv_data_state[7]) << 8);
            // if(game_state_msg.hityaw == 0) 
            //     game_state_msg.hityaw = uint16_t(-1);
        }
        //剩余时间
        // if(recv_data_judge.size() > 0)
        // {
        //     game_state_msg.remain_time = uint16_t(recv_data_judge[3]) | (uint16_t(recv_data_judge[4]) << 8);
        //     game_state_msg.own_alive = recv_data_judge[5] & 0x0f;
        //     game_state_msg.enemy_alive = (recv_data_judge[5] & 0xf0) >> 4;
        // }
        if(recv_data_judge.size() >= 8)
        {
            game_state_msg.outpost_blood = (uint16_t(recv_data_judge[2] >> 4) & 0x0F) | (uint16_t(recv_data_judge[3] ) << 4);
            game_state_msg.base_blood = uint16_t(((uint16_t(recv_data_judge[1]) & 0xff) | (uint16_t(recv_data_judge[2]) & 0x0f) << 8) / 4 * 5);    // 5000血映射到4000
            // std::cout <<"recv_data_judge: "<< std::hex<<recv_data_judge[2] <<",  "<< recv_data_judge[3]<<std::endl;
            
            game_state_msg.game_state = recv_data_judge[0] & 0x07; 
            game_state_msg.RFID_center = (recv_data_judge[0] >> 3) & 0x01;
            game_state_msg.RFID_healing = (recv_data_judge[0] >> 4) & 0x01;
            game_state_msg.remain_time = uint16_t(recv_data_judge[4]) | (uint16_t(recv_data_judge[5] & 0x0F) << 8);
            game_state_msg.coin_remain = uint16_t(((recv_data_judge[5] & 0x0F) >> 4) | ((recv_data_judge[6] & 0xFF) << 4)); //2025 7 1 coin
            game_state_msg.coin_remain = ((game_state_msg.coin_remain & 0x0800) << 4) | (game_state_msg.coin_remain & 0x07FF);

            // 解析第8个字节(recv_data_judge[7])的底盘能量和电容能量信息
            // 底盘能量大于50,默认反馈0x32，特别处置取低6位
            uint8_t chassis_energy_bits = recv_data_judge[7] & 0x3F;
            if (chassis_energy_bits == 0x32) 
            {game_state_msg.chassis_energy_remain = 50;}
            // 正常提取低5位（底盘能量百分比）
            else
            {
                chassis_energy_bits = chassis_energy_bits & 0x1F;
                // 根据位的优先级判断能量百分比（从高到低）
                if (chassis_energy_bits & 0x01) {
                    // bit0为1，能量≥50%
                    game_state_msg.chassis_energy_remain = 50; 
                } else if (chassis_energy_bits & 0x02) {
                    // bit1为1，能量>30%
                    game_state_msg.chassis_energy_remain = 30; 
                } else if (chassis_energy_bits & 0x04) {
                    // bit2为1，能量>15%
                    game_state_msg.chassis_energy_remain = 15; 
                } else if (chassis_energy_bits & 0x08) {
                    // bit3为1，能量>5%
                    game_state_msg.chassis_energy_remain = 5; 
                } else if (chassis_energy_bits & 0x10) {
                    // bit4为1，能量>1%
                    game_state_msg.chassis_energy_remain = 1;  
                } else {
                    // 所有位都为0，能量≤1%
                    game_state_msg.chassis_energy_remain = 0;
                }  
            }
            // 提取高3位（电容能量），实际值为回传*250
            uint8_t cap_energy_value = (recv_data_judge[7] >> 5) & 0x07;
            game_state_msg.cap_energy = cap_energy_value * 250;
        }
        else{
            HINT
        }
        //按鍵信息
        if(recv_data_key.size() > 0)
        {
            KeyMsgPack key_msg_pack;
            key_msg_pack.data[0]    = recv_data_key[6];
            key_msg_pack.data[1]    = recv_data_key[7];
            game_state_msg.key      = key_msg_pack.msg;
        }
        // 客户端信息
        // if(recv_data_client.size() > 0)
        // {
        //     game_state_msg.goal_position_x = (((uint16_t)recv_data_client[0] << 8) | (uint16_t)recv_data_client[1]) / 100.0;
        //     game_state_msg.goal_position_y = (((uint16_t)recv_data_client[2] << 8) | (uint16_t)recv_data_client[3]) / 100.0;
        //     game_state_msg.goal_position_z = (((uint16_t)recv_data_client[4] << 8) | (uint16_t)recv_data_client[5]) / 100.0;
            
        //     game_state_msg.client_key = recv_data_client[6];
        // }
        return game_state_msg;
    }
}
